#pragma once

#include <opencv2\opencv.hpp>

class Screwdriver
{
public:
	float ScrewdriverLength;
public:
	Screwdriver()
	{
		ScrewdriverLength = 0.03+0.04+0.055;
	}

	void GetHeadCoordinate(cv::Vec3f pt[4])
	{
		pt[0] = cv::Vec3f(0, 0, 0);
		pt[1] = cv::Vec3f(0, 0, ScrewdriverLength);
		pt[2] = cv::Vec3f(1, 0, 0);
		pt[3] = cv::Vec3f(0, 1, 0);

		std::cout << "GetHeadCoordinate=" << pt[0] << '\n'
			<< pt[1] << '\n'
			<< pt[2] << '\n'
			<< pt[3] << '\n'
			<< '\n';
	}

	void GetBaseCoordinate(cv::Vec3f pt[4], cv::Vec3f target)
	{
		pt[1] = target;
		pt[0] = target + cv::Vec3f(0, 0, ScrewdriverLength);
		pt[2] = pt[0] + cv::Vec3f(0, 1, 0);
		pt[3] = pt[0] + cv::Vec3f(1, 0, 0);
	}

	cv::Vec3f TipAtBase(cv::Vec6f pos)
	{
		cv::Matx33f rm;
		cv::Rodrigues(cv::Vec3f(pos[3],pos[4],pos[5]), rm);
		return cv::Vec3f(pos[0], pos[1], pos[2]) + rm*cv::Vec3f(0, 0, ScrewdriverLength);
	}


	static void ComputeTransform(cv::Vec3f &xyz, cv::Vec3f &rxyz, const cv::Vec3f ptHead[4], const cv::Vec3f ptBase[4])
	{
		cv::Vec3f ctHead = (ptHead[0] + ptHead[1] + ptHead[2] + ptHead[3]) / 4;
		cv::Vec3f ctBase = (ptBase[0] + ptBase[1] + ptBase[2] + ptBase[3]) / 4;
		cv::Matx33f H = (ptHead[0] - ctHead)*((ptBase[0] - ctBase).t());
		H += (ptHead[1] - ctHead)*((ptBase[1] - ctBase).t());
		H += (ptHead[2] - ctHead)*((ptBase[2] - ctBase).t());
		H += (ptHead[3] - ctHead)*((ptBase[3] - ctBase).t());
		cv::Mat U, S, Vt;
		cv::SVDecomp(H, S, U, Vt);
		cv::Mat R0 = (U*Vt).t();
		cv::Matx33f R((float*)R0.ptr());
		cv::Rodrigues(R, rxyz);
		xyz = ctBase - R*ctHead;
	}

	static cv::Vec6f ComputeTransform(const cv::Vec3f ptFrom[4], const cv::Vec3f ptTo[4])
	{
		cv::Vec3f ctFrom = (ptFrom[0] + ptFrom[1] + ptFrom[2] + ptFrom[3]) / 4;
		cv::Vec3f ctTo = (ptTo[0] + ptTo[1] + ptTo[2] + ptTo[3]) / 4;
		cv::Matx33f H = (ptFrom[0] - ctFrom)*((ptTo[0] - ctTo).t());
		H += (ptFrom[1] - ctFrom)*((ptTo[1] - ctTo).t());
		H += (ptFrom[2] - ctFrom)*((ptTo[2] - ctTo).t());
		H += (ptFrom[3] - ctFrom)*((ptTo[3] - ctTo).t());
		cv::Mat U, S, Vt;
		cv::SVDecomp(H, S, U, Vt);
		cv::Mat R0 = (U*Vt).t();
		cv::Matx33f R((float*)R0.ptr());
		cv::Vec3f rxyz;
		cv::Rodrigues(R, rxyz);
		cv::Vec3f xyz = ctTo - R*ctFrom;
		return cv::Vec6f(xyz[0], xyz[1], xyz[2], rxyz[0], rxyz[1], rxyz[2]);
	}

	void GetArmPos(cv::Vec3f &xyz, cv::Vec3f &rxyz, cv::Vec3f target)
	{
		cv::Vec3f ptHead[4];
		GetHeadCoordinate(ptHead);
		cv::Vec3f ptBase[4];
		GetBaseCoordinate(ptBase, target);
		ComputeTransform(xyz, rxyz, ptHead, ptBase);
	}

	void GetArmPos(cv::Vec6f &pos, cv::Vec3f target)
	{
		cv::Vec3f tmp[2];
		GetArmPos(tmp[0], tmp[1], target);
		pos = cv::Vec6f(tmp[0][0], tmp[0][1], tmp[0][2], tmp[1][0], tmp[1][1], tmp[1][2]);
	}

};